{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 后轮位置反馈实现轨迹跟踪\n",
    "\n",
    "详细讲解参考[博客](https://blog.csdn.net/weixin_42301220/article/details/125003918?spm=1001.2014.3001.5501)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "\n",
    "### 无人车模型\n",
    "假设[无人车模型](https://blog.csdn.net/weixin_42301220/article/details/124747072?spm=1001.2014.3001.5501)如下\n",
    "\n",
    "![在这里插入图片描述](https://img-blog.csdnimg.cn/98de36e913bd4fcd86b4f3ac933b0afc.png)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 21,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "import matplotlib\n",
    "import matplotlib.pyplot as plt\n",
    "# %matplotlib inline\n",
    "# %matplotlib notebook\n",
    "%matplotlib qt5\n",
    "# %matplotlib auto\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 22,
   "metadata": {},
   "outputs": [],
   "source": [
    "import math\n",
    "class KinematicModel_3:\n",
    "  \"\"\"假设控制量为转向角delta_f和加速度a\n",
    "  \"\"\"\n",
    "\n",
    "  def __init__(self, x, y, psi, v, L, dt):\n",
    "    self.x = x\n",
    "    self.y = y\n",
    "    self.psi = psi\n",
    "    self.v = v\n",
    "    self.L = L\n",
    "    # 实现是离散的模型\n",
    "    self.dt = dt\n",
    "\n",
    "  def update_state(self, a, delta_f):\n",
    "    self.x = self.x+self.v*math.cos(self.psi)*self.dt\n",
    "    self.y = self.y+self.v*math.sin(self.psi)*self.dt\n",
    "    self.psi = self.psi+self.v/self.L*math.tan(delta_f)*self.dt\n",
    "    self.v = self.v+a*self.dt\n",
    "\n",
    "  def get_state(self):\n",
    "    return self.x, self.y, self.psi, self.v\n",
    "\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 生成三次样条参考路径曲线\n",
    "\n",
    "使用scipy.interpolate库"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 23,
   "metadata": {},
   "outputs": [],
   "source": [
    "from scipy import interpolate\n",
    "from scipy import optimize\n",
    "class CubicSplinePath:\n",
    "    \"\"\"copied from https://atsushisakai.github.io/PythonRobotics/modules/path_tracking/rear_wheel_feedback_control/rear_wheel_feedback_control.html#rear-wheel-feedback-control\n",
    "    \"\"\"\n",
    "    def __init__(self, x, y):\n",
    "        x, y = map(np.asarray, (x, y))\n",
    "        s = np.append([0],(np.cumsum(np.diff(x)**2) + np.cumsum(np.diff(y)**2))**0.5)\n",
    "\n",
    "        self.X = interpolate.CubicSpline(s, x)\n",
    "        self.Y = interpolate.CubicSpline(s, y)\n",
    "\n",
    "        self.dX = self.X.derivative(1)\n",
    "        self.ddX = self.X.derivative(2)\n",
    "\n",
    "        self.dY = self.Y.derivative(1)\n",
    "        self.ddY = self.Y.derivative(2)\n",
    "\n",
    "        self.length = s[-1]\n",
    "    \n",
    "    def calc_yaw(self, s):\n",
    "        dx, dy = self.dX(s), self.dY(s)\n",
    "        return np.arctan2(dy, dx)\n",
    "    \n",
    "    def calc_curvature(self, s):\n",
    "        \"\"\"计算曲率:设曲线r(t) =(x(t),y(t)),则曲率k=(x'y\" - x\"y')/((x')^2 + (y')^2)^(3/2).\n",
    "          参考：https://blog.csdn.net/weixin_46627433/article/details/123403726\n",
    "        Args:\n",
    "            s (_type_): _description_\n",
    "\n",
    "        Returns:\n",
    "            _type_: 曲率\n",
    "        \"\"\"\n",
    "        dx, dy   = self.dX(s), self.dY(s)\n",
    "        ddx, ddy   = self.ddX(s), self.ddY(s)\n",
    "        return (ddy * dx - ddx * dy) / ((dx ** 2 + dy ** 2)**(3 / 2))\n",
    "    \n",
    "    def __find_nearest_point(self, s0, x, y):\n",
    "        \"\"\"搜寻参考路径与车辆后轴中心距离最近的点：\n",
    "\n",
    "        Args:\n",
    "            s0 (_type_): _description_\n",
    "            x (_type_): _description_\n",
    "            y (_type_): _description_\n",
    "        \"\"\"\n",
    "        def calc_distance(_s, *args):\n",
    "            _x, _y= self.X(_s), self.Y(_s)\n",
    "            return (_x - args[0])**2 + (_y - args[1])**2\n",
    "        \n",
    "        def calc_distance_jacobian(_s, *args):\n",
    "            _x, _y = self.X(_s), self.Y(_s)\n",
    "            _dx, _dy = self.dX(_s), self.dY(_s)\n",
    "            return 2*_dx*(_x - args[0])+2*_dy*(_y-args[1])\n",
    "        # 使用非线性共轭梯度算法使一个函数最小化。\n",
    "        minimum = optimize.fmin_cg(calc_distance, s0, calc_distance_jacobian, args=(x, y), full_output=True, disp=False)\n",
    "        return minimum\n",
    "\n",
    "    def calc_track_error(self, x, y, s0):\n",
    "        \"\"\"计算跟踪误差\n",
    "\n",
    "        Args:\n",
    "            x (_type_): _description_\n",
    "            y (_type_): _description_\n",
    "            s0 (_type_): _description_\n",
    "\n",
    "        Returns:\n",
    "            _type_: _description_\n",
    "        \"\"\"\n",
    "        ret = self.__find_nearest_point(s0, x, y)\n",
    "        \n",
    "        s = ret[0][0]\n",
    "        e = ret[1]\n",
    "\n",
    "        k   = self.calc_curvature(s)\n",
    "        yaw = self.calc_yaw(s)  # 轨迹的航向角，即公式中的psi_e\n",
    "\n",
    "        # 跟踪误差向量d=(dx1,dy1)\n",
    "        dxl = self.X(s) - x\n",
    "        dyl = self.Y(s) - y\n",
    "        angle = normalize_angle(yaw - math.atan2(dyl, dxl))\n",
    "        if angle < 0:\n",
    "            e*= -1\n",
    "\n",
    "        return e, k, yaw, s"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 使用自己的方法生成参考曲线\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 24,
   "metadata": {},
   "outputs": [],
   "source": [
    "class MyReferencePath:\n",
    "    def __init__(self):\n",
    "        # set reference trajectory\n",
    "        # refer_path包括4维：位置x, 位置y， 轨迹点的切线方向, 曲率k \n",
    "        self.refer_path = np.zeros((1000, 4))\n",
    "        self.refer_path[:,0] = np.linspace(0, 100, 1000) # x\n",
    "        self.refer_path[:,1] = 2*np.sin(self.refer_path[:,0]/3.0)+2.5*np.cos(self.refer_path[:,0]/2.0) # y\n",
    "        # 使用差分的方式计算路径点的一阶导和二阶导，从而得到切线方向和曲率\n",
    "        for i in range(len(self.refer_path)):\n",
    "            if i == 0:\n",
    "                dx = self.refer_path[i+1,0] - self.refer_path[i,0]\n",
    "                dy = self.refer_path[i+1,1] - self.refer_path[i,1]\n",
    "                ddx = self.refer_path[2,0] + self.refer_path[0,0] - 2*self.refer_path[1,0]\n",
    "                ddy = self.refer_path[2,1] + self.refer_path[0,1] - 2*self.refer_path[1,1]\n",
    "            elif i == (len(self.refer_path)-1):\n",
    "                dx = self.refer_path[i,0] - self.refer_path[i-1,0]\n",
    "                dy = self.refer_path[i,1] - self.refer_path[i-1,1]\n",
    "                ddx = self.refer_path[i,0] + self.refer_path[i-2,0] - 2*self.refer_path[i-1,0]\n",
    "                ddy = self.refer_path[i,1] + self.refer_path[i-2,1] - 2*self.refer_path[i-1,1]\n",
    "            else:      \n",
    "                dx = self.refer_path[i+1,0] - self.refer_path[i,0]\n",
    "                dy = self.refer_path[i+1,1] - self.refer_path[i,1]\n",
    "                ddx = self.refer_path[i+1,0] + self.refer_path[i-1,0] - 2*self.refer_path[i,0]\n",
    "                ddy = self.refer_path[i+1,1] + self.refer_path[i-1,1] - 2*self.refer_path[i,1]\n",
    "            self.refer_path[i,2]=math.atan2(dy,dx) # yaw\n",
    "            # 计算曲率:设曲线r(t) =(x(t),y(t)),则曲率k=(x'y\" - x\"y')/((x')^2 + (y')^2)^(3/2).\n",
    "            # 参考：https://blog.csdn.net/weixin_46627433/article/details/123403726\n",
    "            self.refer_path[i,3]=(ddy * dx - ddx * dy) / ((dx ** 2 + dy ** 2)**(3 / 2)) # 曲率k计算\n",
    "            \n",
    "    def calc_track_error(self, x, y):\n",
    "        \"\"\"计算跟踪误差\n",
    "\n",
    "        Args:\n",
    "            x (_type_): 当前车辆的位置x\n",
    "            y (_type_): 当前车辆的位置y\n",
    "\n",
    "        Returns:\n",
    "            _type_: _description_\n",
    "        \"\"\"\n",
    "        d_x = [self.refer_path[i,0]-x for i in range(len(self.refer_path))] \n",
    "        d_y = [self.refer_path[i,1]-y for i in range(len(self.refer_path))] \n",
    "        d = [np.sqrt(d_x[i]**2+d_y[i]**2) for i in range(len(d_x))]\n",
    "        s = np.argmin(d)\n",
    "\n",
    "\n",
    "        yaw = self.refer_path[s, 2]\n",
    "        k = self.refer_path[s, 3]\n",
    "        angle = normalize_angle(yaw - math.atan2(d_y[s], d_x[s]))\n",
    "        e = d[s]  # 误差\n",
    "        if angle < 0:\n",
    "            e *= -1\n",
    "\n",
    "        return e, k, yaw, s\n",
    "        \n",
    "        "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 参数列表"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 25,
   "metadata": {},
   "outputs": [],
   "source": [
    "\n",
    "Kpsi=1.0\n",
    "\n",
    "K2=0.5\n",
    "\n",
    "dt=0.1 # 时间间隔，单位：s\n",
    "L=2 # 车辆轴距，单位：m\n",
    "v = 2 # 初始速度\n",
    "x_0=0 # 初始x\n",
    "y_0=-3 #初始y\n",
    "psi_0=0 # 初始航向角"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 角度归一化到[-pi,pi]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 26,
   "metadata": {},
   "outputs": [],
   "source": [
    "def normalize_angle(angle):\n",
    "    \"\"\"\n",
    "    Normalize an angle to [-pi, pi].\n",
    "\n",
    "    :param angle: (float)\n",
    "    :return: (float) Angle in radian in [-pi, pi]\n",
    "    copied from https://atsushisakai.github.io/PythonRobotics/modules/path_tracking/stanley_control/stanley_control.html\n",
    "    \"\"\"\n",
    "    while angle > np.pi:\n",
    "        angle -= 2.0 * np.pi\n",
    "\n",
    "    while angle < -np.pi:\n",
    "        angle += 2.0 * np.pi\n",
    "\n",
    "    return angle\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 后轮位置反馈控制"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 27,
   "metadata": {},
   "outputs": [],
   "source": [
    "def rear_wheel_feedback_control(robot_state, e, k, refer_path_psi):\n",
    "    \"\"\"后轮位置反馈控制\n",
    "\n",
    "    Args:\n",
    "        robot_state (_type_): 机器人位姿，包括x,y,yaw,v\n",
    "        e (_type_): _description_\n",
    "        k (_type_): 曲率\n",
    "        refer_path (_type_): 参考轨迹\n",
    "        refer_path_psi (_type_): 参考轨迹上点的切线方向的角度\n",
    "\n",
    "    Returns:\n",
    "        _type_: _description_\n",
    "    \"\"\"\n",
    "    psi,v = robot_state[2], robot_state[3]\n",
    "    psi_e = normalize_angle(psi - refer_path_psi)  # psi_e=yaw-ref_yaw\n",
    "    # 公式17\n",
    "    psi_dot = v * k * math.cos(psi_e) / (1.0 - k * e)  - K2 * v * math.sin(psi_e) * e / psi_e- Kpsi * abs(v) * psi_e\n",
    "\n",
    "    if psi_e == 0.0 or psi_dot == 0.0:\n",
    "        return 0.0\n",
    "    # 公式21\n",
    "    delta = math.atan2(L * psi_dot, v)\n",
    "\n",
    "    return delta\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 主函数"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 28,
   "metadata": {},
   "outputs": [],
   "source": [
    "from celluloid import Camera # 保存动图时用，pip install celluloid\n",
    "# 使用三次样条生成的曲线\n",
    "def main():\n",
    "\n",
    "    print(\"rear wheel feedback tracking start!!\")\n",
    "    ax = [0.0, 6.0, 12.5, 5.0, 7.5, 3.0, -1.0]\n",
    "    ay = [0.0, 0.0, 5.0, 6.5, 3.0, 5.0, -2.0]\n",
    "    goal = [ax[-1], ay[-1]]\n",
    "    reference_path = CubicSplinePath(ax, ay)\n",
    "    s = np.arange(0, reference_path.length, 0.1)\n",
    "    e, k, yaw_ref, s0 = reference_path.calc_track_error(x_0, y_0, 0.0)\n",
    "\n",
    "\n",
    "\n",
    "    # 运动学模型\n",
    "    ugv = KinematicModel_3(x_0, y_0, psi_0, v, L, dt)\n",
    "    x_ = []\n",
    "    y_ = []\n",
    "    fig = plt.figure(1)\n",
    "    # 保存动图用\n",
    "    camera = Camera(fig)\n",
    "    # plt.ylim([-3,3])\n",
    "    \n",
    "    for i in range(500):\n",
    "        robot_state = np.zeros(4)\n",
    "        robot_state[0] = ugv.x\n",
    "        robot_state[1] = ugv.y\n",
    "        robot_state[2]=ugv.psi\n",
    "        robot_state[3]=ugv.v\n",
    "        e, k, yaw_ref, s0 = reference_path.calc_track_error(robot_state[0], robot_state[1], s0)\n",
    "\n",
    "        delta = rear_wheel_feedback_control(robot_state, e, k, yaw_ref)\n",
    "\n",
    "        ugv.update_state(0, delta)  # 加速度设为0，恒速\n",
    "\n",
    "        x_.append(ugv.x)\n",
    "        y_.append(ugv.y)\n",
    "\n",
    "        # 显示动图\n",
    "        plt.cla()\n",
    "        plt.plot(reference_path.X(s), reference_path.Y(s), \"-.b\",  linewidth=1.0, label=\"course\")\n",
    "        plt.plot(x_, y_, \"-r\", label=\"trajectory\")\n",
    "        plt.plot(reference_path.X(s0), reference_path.Y(s0), \"go\", label=\"target\")\n",
    "        # plt.axis(\"equal\")\n",
    "        plt.grid(True)\n",
    "        plt.pause(0.001)\n",
    "\n",
    "        # camera.snap()\n",
    "        # 判断是否到达最后一个点\n",
    "        if np.linalg.norm(robot_state[0:2]-goal)<=0.1:\n",
    "            print(\"reach goal\")\n",
    "            break\n",
    "    # animation = camera.animate()\n",
    "    # animation.save('trajectory.gif')\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 29,
   "metadata": {},
   "outputs": [],
   "source": [
    "from celluloid import Camera # 保存动图时用，pip install celluloid\n",
    "# 使用自己随便生成的轨迹\n",
    "def main_2():\n",
    "\n",
    "    print(\"rear wheel feedback tracking start!!\")\n",
    "    reference_path = MyReferencePath()\n",
    "    goal = reference_path.refer_path[-1,0:2]\n",
    "\n",
    "\n",
    "\n",
    "    # 运动学模型\n",
    "    ugv = KinematicModel_3(x_0, y_0, psi_0, v, L, dt)\n",
    "    x_ = []\n",
    "    y_ = []\n",
    "    fig = plt.figure(1)\n",
    "    # 保存动图用\n",
    "    camera = Camera(fig)\n",
    "    # plt.ylim([-3,3])\n",
    "    for i in range(500):\n",
    "        robot_state = np.zeros(4)\n",
    "        robot_state[0] = ugv.x\n",
    "        robot_state[1] = ugv.y\n",
    "        robot_state[2]=ugv.psi\n",
    "        robot_state[3]=ugv.v\n",
    "        e, k, yaw_ref, s0 = reference_path.calc_track_error(robot_state[0], robot_state[1])\n",
    "\n",
    "        delta = rear_wheel_feedback_control(robot_state, e, k, yaw_ref)\n",
    "\n",
    "        ugv.update_state(0, delta)  # 加速度设为0，恒速\n",
    "\n",
    "        x_.append(ugv.x)\n",
    "        y_.append(ugv.y)\n",
    "\n",
    "        # 显示动图\n",
    "        plt.cla()\n",
    "        plt.plot(reference_path.refer_path[:,0], reference_path.refer_path[:,1], \"-.b\",  linewidth=1.0, label=\"course\")\n",
    "        plt.plot(x_, y_, \"-r\", label=\"trajectory\")\n",
    "        plt.plot(reference_path.refer_path[s0,0], reference_path.refer_path[s0,1], \"go\", label=\"target\")\n",
    "        # plt.axis(\"equal\")\n",
    "        plt.grid(True)\n",
    "        plt.pause(0.001)\n",
    "\n",
    "        # camera.snap()\n",
    "        # 判断是否到达最后一个点\n",
    "        if np.linalg.norm(robot_state[0:2]-goal)<=0.1:\n",
    "            print(\"reach goal\")\n",
    "            break\n",
    "    # animation = camera.animate()\n",
    "    # animation.save('trajectory.gif')\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 30,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "rear wheel feedback tracking start!!\n"
     ]
    }
   ],
   "source": [
    "# main()\n",
    "main_2()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": []
  }
 ],
 "metadata": {
  "interpreter": {
   "hash": "0c7484b3574347463e16b31029466871583b0d4e5c4ad861e8848f2d3746b4de"
  },
  "kernelspec": {
   "display_name": "Python 3.8.12 ('gobigger')",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.8.12"
  },
  "orig_nbformat": 4
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
